
/*----------------------------------------------------------------------
  File Name          : OpenImu.h
  Author             : Neil Zhang - Gu
  Version            : V1.0
  Date               : 30/3/2020
  Description        : OpenImu Registers Address

  Analog Devices OpenImu is a complete triple axis gyroscope
  and triple axis accelerometer inertial sensing system

  China Applications Support Team.
  china.support@analog.com
----------------------------------------------------------------------

----------------------------------------------------------------------*/
#ifndef _OpenImu_H_
#define _OpenImu_H_

#ifdef __cplusplus
extern "C"
{
#endif
    
#include "filter.h"
#include "stm32f10x.h"
#include "SpiPeripheral.h"
#include "delay.h"
    
/************** Device Register  *******************/
#define OpenImu_X_GYRO_LOW  0X05
#define OpenImu_X_GYRO_HIGH 0X04
#define OpenImu_Y_GYRO_LOW  0X07
#define OpenImu_Y_GYRO_HIGH 0X06
#define OpenImu_Z_GYRO_LOW  0X09
#define OpenImu_Z_GYRO_HIGH 0X08

#define OpenImu_X_ACCL_LOW  0X0B
#define OpenImu_X_ACCL_HIGH 0X0A
#define OpenImu_Y_ACCL_LOW  0X0D
#define OpenImu_Y_ACCL_HIGH 0X0C
#define OpenImu_Z_ACCL_LOW  0X0F
#define OpenImu_Z_ACCL_HIGH 0X0E

#define OpenImu_Board_TEMP_HIGH   0X16 //温度数据
#define OpenImu_Board_TEMP_LOW    0X17 //温度数据

#define OpenImu_Sensor_TEMP_HIGH   0X18 //温度数据
#define OpenImu_Sensor_TEMP_LOW    0X19 //温度数据

#define OpenImu_AcelScaleFactor   0X46
#define OpenImu_RateScaleFactor   0X47

#define OpenImu_PROD_ID_HHIGH  0X56         //device number  3300
#define OpenImu_PROD_ID_LOW    0X57   

#define OpenImu_BURST         0x3E
#define OpenImu_BURST_VG      0x3D
#define OpenImu_BURST_MAG     0x3F



#define ImuReturnDataLen   40
#define F4_Angle_Imu_Order          0x04
#define F4_Zero_Imu_Order           0x05
#define F4_OffestAngle_Imu_Order    0x06

extern struct Task  DriftTask ;
extern u8 CalibrationTimes;  //第二次校准进入中断的次数标志
extern u8 Integr_Flag_1ms;
extern u8 Integr_Flag;
extern u8 Drinft_1S_flag;
extern u16 Drinft_1S_cnt;
extern int Drift_Loop_Cnt ;
static u8  offest_cnt=2;     //  校准次数        
static u8 flag_init=0;       //用于二次校准
static u8 Drift_ave_flag =0;  //用于第二次取静态变量
extern u8 Zero_Status[];   
    
        typedef enum
        {
            Burst32_NumByte = 16,
            Burst16_NumByte = 10
        } Burst_NumByte;   

        typedef struct
        {
            bool  calibration;
        } Imu_Flag;        
        typedef struct
        {
            float LineAcc;
            float LineVel;
            float AngelVel;
            float Angle;
            float Distance;
            float DeltaAngle;
            float DeltaVel;
        } Imu_xyz_Data;
        

        typedef struct
        {
            u16 Burst32_Data[16];
            u16 Burst16_Data[10];
            u8 ImuSendReturnData[ImuReturnDataLen];
            float anglevel;
        } Imu_Original_Data;

        typedef struct
        {
            int Zero_Order_Cnt;
            u8 Zero_Flag[2];
        } Imu_Zero_Status;
         typedef struct
        {
            float Offest_Data;
            int  Original__Data;
         } Imu_F4_Data;
         typedef struct
        {
            u8  wingbot_vel;
         } WingbotData;
         
        typedef struct
        {
            s16  Previous_Drigt[2];
            int  Final_Drift;
            int  Drinft;
            float  Original__Data;
            float  True_Drift;
            float  PreviousTrueDrift;
            int  original_Drift[100] ;
            int  Extreme_Value[2]; 
            float ExtremeFloat[2];
         } Drift_Mode;
         typedef struct
        {
            float sum;
            float anglevel;
            float filteranglevel;
            int   sum_cnt;
            float max;
            float min;
         } UpdateDrift;
         typedef struct
        {
            s16  Previous_Drigt[2];
            int  Final_Drift;
            s16  Drinft;
            float  Original__Data;
            s16  original_Drift[100] ;
            s16  Extreme_Value[2]; 
            Drift_Mode  Bit32_Mode;
            Drift_Mode  XACC_Bit32_Mode;
            Drift_Mode  YACC_Bit32_Mode;
            int   cnt;
            float InitDrift;
            float tempdrift;
        } Imu_Drift_Data;
        typedef struct
        {
            unsigned int PreviuosTime;
            unsigned int NowTime;
            unsigned int SysTimeMsec;
            double SysTimeSec;
            float DeltaTime;
        } TimeSys;
         
        typedef struct
        {   u16  Imu_Id;
            Imu_xyz_Data    X_Axis;
            Imu_xyz_Data    Y_Axis;
            Imu_xyz_Data    Z_Axis;
            Imu_Original_Data Original;
            Imu_Zero_Status ZeroState;
            Imu_F4_Data     F4_data ;
            UpdateDrift     wingbotDrift;
            Imu_Drift_Data  Drift_Data;
            TimeSys            ImuTime;
            float  temperature;
        } Imu_Data;

         typedef struct
        {  
            MovAverage_32Bit   mv_x;
            MovAverage_32Bit   mv_y;
            MovAverage_32Bit   mv_z;
            
            MovAverage_Float   mv_zaxis;
            
            int MfDataX[10];               //因为位移动平均滤波，需存储一个领域内的数据
            int MfDataY[10];
            int MfDataZ[10];
            
            float MfDataMV[10];
        }   Im_Move_Filter;
        
         typedef struct
        {  
            Imu_xyz_Data X_Axis;
            Imu_xyz_Data Y_Axis;
            Imu_xyz_Data Z_Axis;

        }   Imu_Lowpass_Filter;

        typedef enum
        {
            Imu_STATUS_OK = 0,
            Imu_STATUS_ERROR,
            Imu_STATUS_TIMEOUT,
            Imu_STATUS_NOT_IMPLEMENTED
        } ImuStatusTypeDef;

        typedef enum
        {
            OpenImu_BIT16_ACC   = 0x00c1,
            OpenImu_BIT16_ANGLE = 0x01c1,
            OpenImu_BIT32_ACC   = 0x02c1,
            OpenImu_BIT32_ANGLE = 0x03c1
        } Imu_OpenImuReadMode;
    


    class OpenImu
    {
    private:
        SpiPeripheral *_ptrans;
        uint8_t X_isEnabled;
        float X_Last_ODR;
        uint8_t G_isEnabled;
        float G_Last_ODR;
        float g_sensitivity;
        float x_sensitivity;

    public:
        const static u8 OpenImu_Returndata_len = 24; //定义OpenImu返回F4的数据长度
        const static u8 OpenImu_RX_data_len = 40;    //定义OpenImu接收F4的数据长度
        const static u16 device_id = 0x3300; //device id always be 0x6A
        float Calculus_Time  ;  //积分时间为10ms
            
        double Acc_32Bit_Coefficient;         //32位加速度系数
        double AngleVel_32Bit_Coefficient;   //32位角速度单位系数
        float  AngleCoefficient;             //角度系数
        float  TempCoefficient;              //温度单位系数

        double angular_zero_rate; 
        float DriftDataMax;                   //静态偏移量最大值
        float DriftDataMin;                   //静态偏移量最小值
        Imu_Data OpenImu_Data;
        Im_Move_Filter       Movaverage;          //移动平均滤波结构体
        Imu_Lowpass_Filter   LowpassFilter;       //低通滤波结构体
        Imu_Flag             ImuFlag;             //IMU标志结构体，待补充
        WingbotData wingbot;                      //F4发送过来的小车速度标志
    public:
        static OpenImu *get(void);
        bool init(void);
        void init_static_drift(void);
        u16 readRegister(u16 regaddr);
        u16 writeRegister(uint16_t pBuffer, u16 RegisterAddr);
        u16 getDeviceId(void);
        void refresh_zero_rate(void);
        void OpenImu_Init(void); //IO初始化 
        u8   SetMode(bool mode) ;
        u8   GetData();
   
        void ImuResestInit();
        void ImuResest();
        u8  UartanalysisImuData();
        u8  GetExtreme(float *data);
        ImuStatusTypeDef ImuCalibration_Uart(int times);    //Imu校准数据，获得静态漂移量
        ImuStatusTypeDef UartGetAngleVel(Queue *queue) ;   //Imu校准数据，获得静态漂移量
        float ImuUpdateDriftUart(float *vel);

        ImuStatusTypeDef BurstReadRegister(u16 RegisterAddr);
        ImuStatusTypeDef ImuAccDataConversion(u16 *OriginalData);   //原始数据的值进行转化
        ImuStatusTypeDef ImuData_Clear();   //Imu数据清零
        ImuStatusTypeDef ImuGet_Drift(int* Bit32_Drift);      //Imu获取数据静态漂移量
        ImuStatusTypeDef ImuCalibration(int times);           //Imu校准数据，获得静态漂移量  
        ImuStatusTypeDef ImuCheckCalibration(float *drift);               //Imu获取到静态偏移量后检查偏移量是否正确  
        float ImuUpdateDrift();
        float ImuGetZaxisAnglevel(bool type);               //Imu得到Z轴角速度  
        float ImuGetTemperature();                 //Imu得到温度 
        float ImuGetTempDrift();                   //Imu得到温度漂移
        ImuStatusTypeDef ImuAcc_32Bit_DataHandle(u16 *OriginalData);   //原始32位数据的值进行转化
        
        /*************************Imu与F4进行数据交互******************************/
        u8 getWingbotVel(u8 *hostdata);
        ImuStatusTypeDef OpenImu_Return_F4_PositionData(u8 *order); //IMU通过串口返回F4实时位置和角度数据的函数,此函数在串口接收到F4指令后调用（参数为F4下发的具体指令）
        ImuStatusTypeDef OpenImu_Return_F4_ZeroData();              //IMU通过串口返回F4零点位置是否设置成功的函数
        ImuStatusTypeDef OpenImu_Analysis_F4_Order(u8 *order);      //在解析到F4下发的指令后解析指令的具体内容,参数为F4下发的串口数据
        ImuStatusTypeDef OpenImu_F4_Offest_Position(u8 *order);      //在解析到F4下发的指令后,执行F4下发的校准指令，内容位修正的角度位置
        
        /***************************************************************************/


    private:
     OpenImu(SpiPeripheral *trans);

    };

#ifdef __cplusplus
}
#endif

#endif
